/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#define  PIX_HIST_BIN_NUM_1  3 //number of bins for classification (not used now)
#define  PIX_HIST_BIN_NUM_2  5 //number of bins for statistic collection
#define  PIX_HIST_ALPHA      0.01f //alpha-coefficient for running avarage procedure
#define  PIX_HIST_DELTA      2 //maximal difference between descriptors(RGB)
#define  PIX_HIST_COL_QUANTS 64 //quantization level in rgb-space
#define  PIX_HIST_DELTA_IN_PIX_VAL  (PIX_HIST_DELTA * 256 / PIX_HIST_COL_QUANTS) //allowed difference in rgb-space

// Structures for background statistics estimation:
typedef struct CvPixHistBin {
	float          bin_val;
	uchar          cols[3];
} CvPixHistBin;

typedef struct CvPixHist {
	CvPixHistBin   bins[PIX_HIST_BIN_NUM_2];
} CvPixHist;

// Class for background statistics estimation:
class CvBGEstimPixHist {
private:
	CvPixHist*  m_PixHists;
	int         m_width;
	int         m_height;

	// Function for update color histogram for one pixel:
	void update_hist_elem(int x, int y, uchar* cols ) {
		// Find closest bin:
		int    dist = 0, min_dist = 2147483647, indx = -1;
		for ( int k = 0; k < PIX_HIST_BIN_NUM_2; k++ ) {

			uchar* hist_cols = m_PixHists[y*m_width+x].bins[k].cols;

			m_PixHists[y* m_width+x].bins[k].bin_val *= (1 - PIX_HIST_ALPHA);

			int  l;
			for ( l = 0; l < 3; l++ ) {
				int val = abs( hist_cols[l] - cols[l] );
				if ( val > PIX_HIST_DELTA_IN_PIX_VAL ) { break; }
				dist += val;
			}

			if ( l == 3 && dist < min_dist ) {
				min_dist = dist;
				indx = k;
			}
		}
		if ( indx < 0 ) { // N2th elem in the table is replaced by a new feature.
			indx = PIX_HIST_BIN_NUM_2 - 1;
			m_PixHists[y* m_width+x].bins[indx].bin_val = PIX_HIST_ALPHA;
			for (int l = 0; l < 3; l++ ) {
				m_PixHists[y* m_width+x].bins[indx].cols[l] = cols[l];
			}
		} else {
			//add vote!
			m_PixHists[y* m_width+x].bins[indx].bin_val += PIX_HIST_ALPHA;
		}
		// Re-sort bins by BIN_VAL:
		{
			int k;
			for (k = 0; k < indx; k++ ) {
				if ( m_PixHists[y* m_width+x].bins[k].bin_val <= m_PixHists[y*m_width+x].bins[indx].bin_val ) {
					CvPixHistBin tmp1, tmp2 = m_PixHists[y*m_width+x].bins[indx];
					// Shift elements:
					for (int l = k; l <= indx; l++ ) {
						tmp1 = m_PixHists[y*m_width+x].bins[l];
						m_PixHists[y* m_width+x].bins[l] = tmp2;
						tmp2 = tmp1;
					}
					break;
				}
			}
		}
	}   // void update_hist(...)

	// Function for calculation difference between histograms:
	float get_hist_diff(int x1, int y1, int x2, int y2) {
		float  dist = 0;
		for ( int i = 0; i < 3; i++ ) {
			dist += labs(m_PixHists[y1*m_width+x1].bins[0].cols[i] -
						 m_PixHists[y2*m_width+x2].bins[0].cols[i]);
		}
		return dist;
	}


public:
	IplImage*   bg_image;

	CvBGEstimPixHist(CvSize img_size) {
		m_PixHists = (CvPixHist*)cvAlloc(img_size.width * img_size.height * sizeof(CvPixHist));
		memset( m_PixHists, 0, img_size.width * img_size.height * sizeof(CvPixHist) );
		m_width = img_size.width;
		m_height = img_size.height;

		bg_image = cvCreateImage(img_size, IPL_DEPTH_8U, 3 );
	}   /* Constructor. */

	~CvBGEstimPixHist() {
		cvReleaseImage(&bg_image);
		cvFree(&m_PixHists);
	}   /* Destructor. */

	// Function to update histograms and bg_image:
	void update_hists( IplImage* pImg ) {
		for ( int i = 0; i < pImg->height; i++ ) {
			for ( int j = 0; j < pImg->width; j++ ) {
				update_hist_elem( j, i, ((uchar*)(pImg->imageData)) + i * pImg->widthStep + 3 * j );
				((uchar*)(bg_image->imageData))[i* bg_image->widthStep+3*j] = m_PixHists[i*m_width+j].bins[0].cols[0];
				((uchar*)(bg_image->imageData))[i* bg_image->widthStep+3*j+1] = m_PixHists[i*m_width+j].bins[0].cols[1];
				((uchar*)(bg_image->imageData))[i* bg_image->widthStep+3*j+2] = m_PixHists[i*m_width+j].bins[0].cols[2];
			}
		}
		// cvNamedWindow("RoadMap2",0);
		// cvShowImage("RoadMap2", bg_image);
	}
};  /* CvBGEstimPixHist */



/*======================= TRACKER LIST SHELL =====================*/
typedef struct DefBlobTrackerL {
	CvBlob                  blob;
	CvBlobTrackerOne*       pTracker;
	int                     Frame;
	int                     Collision;
	CvBlobTrackPredictor*   pPredictor;
	CvBlob                  BlobPredict;
	CvBlobSeq*              pBlobHyp;
} DefBlobTrackerL;

class CvBlobTrackerList : public CvBlobTracker {
private:
	CvBlobTrackerOne*       (*m_Create)();
	CvBlobSeq               m_BlobTrackerList;
//    int                     m_LastID;
	int                     m_Collision;
	int                     m_ClearHyp;
	float                   m_BGImageUsing;
	CvBGEstimPixHist*       m_pBGImage;
	IplImage*               m_pImgFG;
	IplImage*               m_pImgReg; /* mask for multiblob confidence calculation */

public:
	CvBlobTrackerList(CvBlobTrackerOne* (*create)()): m_BlobTrackerList(sizeof(DefBlobTrackerL)) {
		//int i;
		CvBlobTrackerOne* pM = create();
//        m_LastID = 0;
		m_Create = create;
		m_ClearHyp = 0;
		m_pImgFG = 0;
		m_pImgReg = NULL;

		TransferParamsFromChild(pM, NULL);

		pM->Release();

		m_Collision = 1; /* if 1 then collistion will be detected and processed */
		AddParam("Collision", &m_Collision);
		CommentParam("Collision", "if 1 then collision cases are processed in special way");

		m_pBGImage = NULL;
		m_BGImageUsing = 50;
		AddParam("BGImageUsing", &m_BGImageUsing);
		CommentParam("BGImageUsing", "Weight of using BG image in update hist model (0 - BG dies not use 1 - use)");

		SetModuleName("List");
	}

	~CvBlobTrackerList() {
		int i;
		if (m_pBGImage) { delete m_pBGImage; }
		if (m_pImgFG) { cvReleaseImage(&m_pImgFG); }
		if (m_pImgReg) { cvReleaseImage(&m_pImgReg); }
		for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
			m_BlobTrackerList.DelBlob(i - 1);
		}
	};

	CvBlob* AddBlob(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG ) {
		/* Create new tracker: */
		DefBlobTrackerL F;
		F.blob = pBlob[0];
//        F.blob.ID = m_LastID++;
		F.pTracker = m_Create();
		F.pPredictor = cvCreateModuleBlobTrackPredictKalman();
		F.pBlobHyp = new CvBlobSeq;
		F.Frame = 0;
		TransferParamsToChild(F.pTracker, NULL);

		F.pTracker->Init(pBlob, pImg, pImgFG);
		m_BlobTrackerList.AddBlob((CvBlob*)&F);
		return m_BlobTrackerList.GetBlob(m_BlobTrackerList.GetBlobNum() - 1);
	};

	void DelBlob(int BlobIndex) {
		DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
		if (pF == NULL) { return; }
		pF->pTracker->Release();
		pF->pPredictor->Release();
		delete pF->pBlobHyp;
		m_BlobTrackerList.DelBlob(BlobIndex);
	}

	void DelBlobByID(int BlobID) {
		DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(BlobID);
		if (pF == NULL) { return; }
		pF->pTracker->Release();
		pF->pPredictor->Release();
		delete pF->pBlobHyp;
		m_BlobTrackerList.DelBlobByID(BlobID);
	}

	virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL) {
		int i;
		if (pImgFG) {
			if (m_pImgFG) { cvCopy(pImgFG, m_pImgFG); }
			else { m_pImgFG = cvCloneImage(pImgFG); }
		}

		if (m_pBGImage == NULL && m_BGImageUsing > 0) {
			m_pBGImage = new CvBGEstimPixHist(cvSize(pImg->width, pImg->height));
		}

		if (m_Collision)
			for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
				/* Update predictor: */
				DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i - 1);
				pF->pPredictor->Update((CvBlob*)pF);
			}   /* Update predictor. */

		if (m_pBGImage && m_pImgFG) {
			/* Weighting mask mask: */
			int x, y, yN = pImg->height, xN = pImg->width;
			IplImage* pImgBG = NULL;
			m_pBGImage->update_hists(pImg);
			pImgBG = m_pBGImage->bg_image;

			for (y = 0; y < yN; ++y) {
				unsigned char* pI = (unsigned char*)pImg->imageData + y * pImg->widthStep;
				unsigned char* pBG = (unsigned char*)pImgBG->imageData + y * pImgBG->widthStep;
				unsigned char* pFG = (unsigned char*)m_pImgFG->imageData + y * m_pImgFG->widthStep;

				for (x = 0; x < xN; ++x) {
					if (pFG[x]) {
						int D1 = (int)(pI[3*x+0]) - (int)(pBG[3*x+0]);
						int D2 = (int)(pI[3*x+1]) - (int)(pBG[3*x+1]);
						int D3 = (int)(pI[3*x+2]) - (int)(pBG[3*x+2]);
						int DD = D1 * D1 + D2 * D2 + D3 * D3;
						double  D = sqrt((float)DD);
						double  DW = 25;
						double  W = 1 / (exp(-4 * (D - m_BGImageUsing) / DW) + 1);
						pFG[x] = (uchar)cvRound(W * 255);
					}
				}   /* Next mask pixel. */
			}   /*  Next mask line. */
			/*if(m_Wnd)
			{
			    cvNamedWindow("BlobList_FGWeight",0);
			    cvShowImage("BlobList_FGWeight",m_pImgFG);
			}*/
		}   /* Weighting mask mask. */

		for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
			/* Predict position. */
			DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i - 1);
			CvBlob*         pB = pF->pPredictor->Predict();
			if (pB) {
				pF->BlobPredict = pB[0];
				pF->BlobPredict.w = pF->blob.w;
				pF->BlobPredict.h = pF->blob.h;
			}
		}   /* Predict position. */

		if (m_Collision)
			for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
				/* Predict collision. */
				int             Collision = 0;
				int             j;
				DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i - 1);

				for (j = m_BlobTrackerList.GetBlobNum(); j > 0; --j) {
					/* Predict collision. */
					CvBlob* pB1;
					CvBlob* pB2;
					DefBlobTrackerL* pF2 = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(j - 1);
					if (i == j) { continue; }
					pB1 = &pF->BlobPredict;
					pB2 = &pF2->BlobPredict;
					if ( fabs(pB1->x - pB2->x) < 0.5 * (pB1->w + pB2->w) &&
							fabs(pB1->y - pB2->y) < 0.5 * (pB1->h + pB2->h) ) { Collision = 1; }
					pB1 = &pF->blob;
					pB2 = &pF2->blob;
					if ( fabs(pB1->x - pB2->x) < 0.5 * (pB1->w + pB2->w) &&
							fabs(pB1->y - pB2->y) < 0.5 * (pB1->h + pB2->h) ) { Collision = 1; }
					if (Collision) { break; }
				}   /* Check next blob to cross current. */

				pF->Collision = Collision;
				pF->pTracker->SetCollision(Collision);

			}   /* Predict collision. */

		for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
			/* Track each blob. */
			DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i - 1);
			if (pF->pBlobHyp->GetBlobNum() > 0) {
				/* Track all hypothesis. */
				int h, hN = pF->pBlobHyp->GetBlobNum();
				for (h = 0; h < hN; ++h) {
					CvBlob*     pB = pF->pBlobHyp->GetBlob(h);
					CvBlob*     pNewBlob = pF->pTracker->Process(pB, pImg, m_pImgFG);
					int         BlobID = CV_BLOB_ID(pB);
					if (pNewBlob) {
						pB[0] = pNewBlob[0];
						pB->w = MAX(CV_BLOB_MINW, pNewBlob->w);
						pB->h = MAX(CV_BLOB_MINH, pNewBlob->h);
						CV_BLOB_ID(pB) = BlobID;
					}
				}   /* Next hypothesis. */

			}   /* Track all hypotheses. */

			pF->Frame++;

		}   /* Next blob. */

#if 0
		for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
			/* Update predictor: */
			DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i - 1);
			if ((m_Collision && !pF->Collision) || !m_Collision) {
				pF->pPredictor->Update((CvBlob*)pF);
			} else {
				/* pravilnyp putem idete tovarischy!!! */
				pF->pPredictor->Update(&(pF->BlobPredict));
			}
		}   /* Update predictor. */
#endif
		m_ClearHyp = 1;
	};


	/* Process on blob (for multi hypothesis tracing) */
	virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL) {
		int                 ID = pBlob->ID;
		DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
		CvBlob*             pNewBlob = pF->pTracker->Process(pBlob ? pBlob : &(pF->blob), pImg, m_pImgFG);
		if (pNewBlob) {
			pF->blob = pNewBlob[0];
			pF->blob.w = MAX(CV_BLOB_MINW, pNewBlob->w);
			pF->blob.h = MAX(CV_BLOB_MINH, pNewBlob->h);
			pBlob[0] = pF->blob;
		}
		pBlob->ID = ID;
	};

	virtual double  GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL) {
		DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
		if (pF == NULL) { return 0; }
		if (pF->pTracker == NULL) { return 0; }
		return pF->pTracker->GetConfidence(pBlob ? pBlob : (&pF->blob), pImg, pImgFG, NULL);
	};

	virtual double GetConfidenceList(CvBlobSeq* pBlobList, IplImage* pImg, IplImage* pImgFG = NULL) {
		double  W = 1;
		int     b, bN = pBlobList->GetBlobNum();

		if (m_pImgReg == NULL) {
			m_pImgReg = cvCreateImage(cvSize(pImg->width, pImg->height), IPL_DEPTH_8U, 1);
		}
		assert(pImg);

		cvSet(m_pImgReg, cvScalar(255));

		for (b = 0; b < bN; ++b) {
			CvBlob* pB = pBlobList->GetBlob(b);
			DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(pB->ID);
			if (pF == NULL || pF->pTracker == NULL) { continue; }
			W *= pF->pTracker->GetConfidence(pB, pImg, pImgFG, m_pImgReg );
			cvEllipse(
				m_pImgReg,
				cvPoint(cvRound(pB->x * 256), cvRound(pB->y * 256)), cvSize(cvRound(pB->w * 128), cvRound(pB->h * 128)),
				0, 0, 360,
				cvScalar(0), CV_FILLED, 8, 8 );
//            cvNamedWindow("REG",0);
//            cvShowImage("REG",m_pImgReg);
//            cvWaitKey(0);
		}
		return W;
	};

	virtual void UpdateBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL) {
		DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
		if (pF) {
			pF->pTracker->Update(pBlob ? pBlob : &(pF->blob), pImg, m_pImgFG);
		}
	};

	int     GetBlobNum() {return m_BlobTrackerList.GetBlobNum();};
	CvBlob* GetBlob(int index) {return m_BlobTrackerList.GetBlob(index);};

	void  SetBlob(int BlobIndex, CvBlob* pBlob) {
		CvBlob* pB = m_BlobTrackerList.GetBlob(BlobIndex);
		if (pB) {
			pB[0] = pBlob[0];
			pB->w = MAX(CV_BLOB_MINW, pBlob->w);
			pB->h = MAX(CV_BLOB_MINH, pBlob->h);
		}
	}

	void    Release() {delete this;};

	/* Additional functionality: */
	CvBlob* GetBlobByID(int BlobID) {return m_BlobTrackerList.GetBlobByID(BlobID);}

	/*  ===============  MULTI HYPOTHESIS INTERFACE ==================  */
	/* Return number of position hypotheses of currently tracked blob: */
	virtual int     GetBlobHypNum(int BlobIdx) {
		DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIdx);
		assert(pF->pBlobHyp);
		return pF->pBlobHyp->GetBlobNum();
	};  /* CvBlobtrackerList::GetBlobHypNum() */

	/* Return pointer to specified blob hypothesis by index blob: */
	virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis) {
		DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
		assert(pF->pBlobHyp);
		return pF->pBlobHyp->GetBlob(hypothesis);
	};  /* CvBlobtrackerList::GetBlobHyp() */

	/* Set new parameters for specified (by index) blob hyp (can be called several times for each hyp )*/
	virtual void    SetBlobHyp(int BlobIndex, CvBlob* pBlob) {
		if (m_ClearHyp) {
			/* Clear all hypotheses: */
			int b, bN = m_BlobTrackerList.GetBlobNum();
			for (b = 0; b < bN; ++b) {
				DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(b);
				assert(pF->pBlobHyp);
				pF->pBlobHyp->Clear();
			}
			m_ClearHyp = 0;
		}
		{
			/* Add hypothesis: */
			DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
			assert(pF->pBlobHyp);
			pF->pBlobHyp->AddBlob(pBlob);
		}
	};  /* CvBlobtrackerList::SetBlobHyp */

private:
public:
	void ParamUpdate() {
		int i;
		for (i = m_BlobTrackerList.GetBlobNum(); i > 0; --i) {
			DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i - 1);
			TransferParamsToChild(pF->pTracker);
			pF->pTracker->ParamUpdate();
		}
	}
};  /* CvBlobTrackerList */

CvBlobTracker* cvCreateBlobTrackerList(CvBlobTrackerOne* (*create)()) {
	return (CvBlobTracker*) new CvBlobTrackerList(create);
}
